/*+***********************************************************
Filename: vectors.S
Description: reset entry.
Modification:
2023.04.21 hzheng creation
2023.08.26 hzheng Modify isr_wrapper, use ecall and timer3 int
                   for task switch. Modify push_reg and pop_reg.
************************************************************-*/


### my reset entry

    .section .text.my_entry
    .type my_entry_start, @function
my_entry_start:

## call app_main
    la     t0, app_main
    jalr   t0
     

     
##
## flash section
##
    .section .text.my_flash_entry
    .global _start
    .type _start, @function
_start:
## set led on
    lui  a5,0x50006
    li      s0,4
    sh      s0,4(a5)   #GPIO_DIR=0x50006004
    li      s0,4       #0 or 4
    sh      s0,0(a5)   #GPIO_ODR=0x50006000

## set sp
    la gp, __exc_stack_top
    la sp, __SYSTEM_STACK_END__

## clear status reg
    csrwi  mstatus,0
    csrwi  mie,0

## set interrupt vector
#    la t0, trap_vector+1
# non-vector mode
    la t0, trap_vector
    csrw	mtvec,t0

## enable core interrupt
#    li t0, 0x80
    li t0, 0x888
    csrw mie,t0

    
    
## clear pmp config
    lui	t0,0
    csrw	pmpcfg0,t0
    csrw	pmpcfg1,t0
    csrw	pmpcfg2,t0
    csrw	pmpcfg3,t0

## shutdown watchdog
    lui a4,0x50000       #0x5000_0048
    lhu a5,48(a4)
    slli    a5,a5,0x10
    srli    a5,a5,0x10
    andi    a5,a5,-2
    slli    a5,a5,0x10
    srli    a5,a5,0x10
    sh  a5,48(a4)
    
    lui	a4,0x40000
    lw	a5,0(a4)
    andi	a5,a5,-3
    sw	a5,0(a4)
    
## init special csr
    csrwi  0x7c0,0
    fence
    csrwi  0x7c1,0
    fence
    csrwi	0x7ef,1
    fence

## copy c style inited data
copy_data_to_ram:
    la t0,__data_load
    la t1,__data_start
    la t2,__data_end
    beq t1,t2,copy_finished
copy_next:
    lw t3,0(t0)
    sw t3,0(t1)
    addi t0,t0,4
    addi t1,t1,4
    bne t1,t2,copy_next
copy_finished:

## Clear bss section
    la t0, __bss_begin
    la t1, __bss_end
    bgeu t0, t1, clr_bss_finished
clr_bss_next:
    sw zero, (t0)
    addi t0, t0, 4
    bltu t0, t1, clr_bss_next
clr_bss_finished:

    
## enable global mie
    csrsi  mstatus,0x08

    
## call my_main
    la     t0, my_main
    jalr   t0
    j .  #dead loop
    

## ISR and trap vector

#macro for save and restore regs
#   
.macro push_reg
    addi  sp, sp, -(32 * 4)
    ##offset 0: sp
    sw sp, 0 * 4(sp)
    sw tp, 1 * 4(sp)
    sw t6, 2 * 4(sp)
    sw t5, 3 * 4(sp)
    sw t4, 4 * 4(sp)
    sw t3, 5 * 4(sp)
    sw t2, 6 * 4(sp)
    sw t1, 7 * 4(sp)
    sw t0, 8 * 4(sp)
    sw s11, 9 * 4(sp)
    sw s10, 10 * 4(sp)
    sw s9, 11 * 4(sp)
    sw s8, 12 * 4(sp)
    sw s7, 13 * 4(sp)
    sw s6, 14 * 4(sp)
    sw s5, 15 * 4(sp)
    ##offset 16: mstatus
    csrr t0, mstatus
    sw t0, 16 * 4(sp)
    ##offset 17: mepc
    csrr t0, mepc
    sw t0, 17 * 4(sp)
    lw t0, 8 * 4(sp)
    #
    sw a7, 18 * 4(sp)
    sw a6, 19 * 4(sp)
    sw a5, 20 * 4(sp)
    sw a4, 21 * 4(sp)
    sw a3, 22 * 4(sp)
    sw a2, 23 * 4(sp)
    sw a1, 24 * 4(sp)
    sw a0, 25 * 4(sp)
    sw s4, 26 * 4(sp)
    sw s3, 27 * 4(sp)
    sw s2, 28 * 4(sp)
    sw s1, 29 * 4(sp)
    sw s0, 30 * 4(sp)
    sw ra, 31 * 4(sp)
.endm

.macro pop_reg
    ##offset 0: sp
    lw tp, 1 * 4(sp)
    lw t6, 2 * 4(sp)
    lw t5, 3 * 4(sp)
    lw t4, 4 * 4(sp)
    lw t3, 5 * 4(sp)
    lw t2, 6 * 4(sp)
    lw t1, 7 * 4(sp)
    lw t0, 8 * 4(sp)
    lw s11, 9 * 4(sp)
    lw s10, 10 * 4(sp)
    lw s9, 11 * 4(sp)
    lw s8, 12 * 4(sp)
    lw s7, 13 * 4(sp)
    lw s6, 14 * 4(sp)
    lw s5, 15 * 4(sp)
    ##offset 16: mstatus
    ##offset 17: mepc
    lw a7, 18 * 4(sp)
    lw a6, 19 * 4(sp)
    lw a5, 20 * 4(sp)
    lw a4, 21 * 4(sp)
    lw a3, 22 * 4(sp)
    lw a2, 23 * 4(sp)
    lw a1, 24 * 4(sp)
    lw a0, 25 * 4(sp)
    lw s4, 26 * 4(sp)
    lw s3, 27 * 4(sp)
    lw s2, 28 * 4(sp)
    lw s1, 29 * 4(sp)
    lw s0, 30 * 4(sp)
    lw ra, 31 * 4(sp)
    addi  sp, sp, 32 * 4
.endm

#myisr should be implemented in other code    
    .global myisr  
    .global switchCurrentTask  
    .global showMsg  

    .type isr_wrapper, @function
isr_wrapper:
#    csrci  mstatus,0x08
    push_reg

    #check mcause
    csrr s1, mcause
    
    #show mcause
#    li a0, 1
#    mv a1, s1
#    jal showMsg
    #show mstatus
#    csrr a1, mstatus
#    li a0, 2
#    jal showMsg
    #
    
    li t1, 11
    bne s1, t1, isr_next
    #ecall handler
#    jal switchCurrentTask
    la s0, g_losTask    #s0 points to g_losTask
    lw	s1, 0(s0) # pointer of runTask  
    lw	s2, 4(s0) # pointer of newTask
    
    #check s1&s2
#    li a0, 3
#    mv a1, s1
#    jal showMsg
#    li a0, 4
#    mv a1, s2
#    jal showMsg
    #

    beq s1, s2, isr_ecall_sametask
    beqz s1, isr_ecall_starttask
    #save runTask's sp and mepc
    sw sp, 0(s1) # save runTask's sp
    csrr t1, mepc
    addi t1, t1, 4
    sw t1, 17 * 4(sp) # save runTask's breakpoint(mepc+4)
isr_ecall_starttask:
    sw s2, 0(s0)  # set runTask = newTask
    lw sp, 0(s2)  # change sp to newTask
    
    #check sp
#    li a0, 5
#    mv a1, sp
#    jal showMsg
    #
    lw t1, 17 * 4(sp)  # get newTask's mepc from stack
    csrw mepc, t1
    
    #check mepc
#    li a0, 6
#    mv a1, t1
#    jal showMsg
    #    
    
    j isr_ret
isr_ecall_sametask:
    #mark
#    li a0, 10
#    li a1, 1
#    jal showMsg
    #

    # mepc+=4
    csrr t1, mepc
    addi t1, t1, 4
    csrw mepc, t1
    j isr_ret
isr_next:  
    #mark
#    li a0, 10
#    li a1, 2
#    jal showMsg
    #
    li t1, 0x80000007 #timer3 int(mtie)
    bne s1, t1, isr_next2
    ##
    ## mtie handler
    li t2, 0x40050048 #TIMER3_EOI
    lw t1, 0(t2)  # clear timer3 int flag
    # set gTimer3IrqFlag = 1
    la t2, gTimer3IrqFlag
    li t1, 1
    sw t1, 0(t2) 
#    jal switchCurrentTask
    jal OsTickHandler
    la s0, g_losTask    #s0 points to g_losTask
    lw	t1,0(s0) # pointer of runTask  
    lw	t2,4(s0) # pointer of newTask
    beq t1, t2, isr_ret #same task, direct ret, no need increase mepc for timer int
    sw sp, 0(t1) # save runTask's sp
    #runTask's mepc is already save in push_reg
    sw t2,0(s0)  # set runTask = newTask
    lw sp,0(t2)  # restore newTask's sp
    lw t1,17 * 4(sp)  # restore newTask's breakpoint from stack
    csrw mepc, t1     # and write to mpec
    # clear gTimer3IrqFlag
    la t2, gTimer3IrqFlag
    li t1, 0
    sw t1, 0(t2) 
    j isr_ret    
    
isr_next2: 
    #mark
#    li a0, 10
#    li a1, 3
#    jal showMsg
    #
    jal   myisr
isr_ret:   
    li a0, 0x80
    csrrs x0, mstatus, a0    
    pop_reg
#    csrsi  mstatus,0x08
    mret
    
    .align 2
trap_vector:
    j isr_wrapper


